home *** CD-ROM | disk | FTP | other *** search
/ CD-ROM Today - The Disc! 5 / CD-ROM Today - The Disc (Issue 5)(November 1994).ISO / mac / Mac shareware / Education / RLaB / examples / lqrtest.r < prev    next >
Text File  |  1994-09-21  |  2KB  |  78 lines
  1.  
  2. printf( "Starting the lqr test...\n" );
  3. rfile ode23
  4.  
  5. lqr = function( a, b, q, r ) {
  6.     local( k, s,...
  7.            m, n, mb, nb, mq, nq,...
  8.            e, v, d );
  9.  
  10.     m = size(a)[1]; n = size(a)[2];
  11.     mb = size(b)[1]; nb = size(b)[2];
  12.     mq = size(q)[1]; nq = size(q)[2];
  13.     
  14.     if ( m != mq || n != nq ) {
  15.     fprintf( "stderr", "A and Q must be the same size.\n" );
  16.     quit
  17.     }
  18.     
  19.     mr = size(r)[1]; nr = size(r)[2];
  20.     if ( mr != nr || nb != mr ) {
  21.     fprintf( "stderr", "B and R must be consistent.\n" );
  22.     quit
  23.     }
  24.     
  25.     nn = zeros( m, nb );
  26.     
  27.     // Start eigenvector decomposition by finding eigenvectors of Hamiltonian:
  28.     
  29.     e = eig( [ a, solve(r',b')'*b'; q, -a' ] );
  30.     v = e.vec; d = e.val;
  31.     
  32.     index = sort( real( d ) ).ind;
  33.     d = real( d[ index ] );
  34.     
  35.     if ( !( d[n] < 0 && d[n+1] > 0 ) ) {
  36.     fprintf( "stderr", "Can't order eigenvalues.\n" );
  37.     quit
  38.     }
  39.     
  40.     chi = v[ 1:n; index[1:n] ];
  41.     lambda = v[ (n+1):(2*n); index[1:n] ];
  42.     s = -real(solve(chi',lambda')');
  43.     k = solve( r, nn'+b'*s );
  44.     
  45.     return << k=k; s=s >>;
  46.  
  47. };
  48.  
  49.  
  50. // Now run a little test problem.
  51.  
  52. k = 1; m = 1; c = .1;
  53. a = [    0,    1,    0,    0; -k/m, -c/m,  k/m,  c/m; 0,    0,    0,    1; k/m,  c/m, -k/m, -c/m ];
  54. b = [ 0; 1/m; 0; 0 ];
  55. qxx = diag( [0, 0, 100, 0] );
  56. ruu = [1];
  57.  
  58. K = lqr( a, b, qxx, ruu ).k;
  59.  
  60. dot = function( t, x ) 
  61. {
  62.  return (a-b*K)*x + b*K*([1,0,1,0]')
  63. };
  64.  
  65. x = ode( dot, 0, 15, [0,0,0,0]',.01, 1.e-5, 1.e-5);
  66.  
  67. m = maxi( x[;2] );
  68.  
  69. if ( (abs( x[m;2] - 1.195 ) > 0.001)  || ...
  70.      any (abs( x[x.nr;2,4] - 1 ) > 0.001) ) {
  71.      printf( "...failed.\a\n" );
  72. else
  73.      plgrid ();
  74.      ptitle ("LQR Test Results");
  75.      plot (x);
  76.      printf( "...passed.\n" );
  77. }
  78.